#include "WProgram.h"
void setup();
void cmdFin();
void cmdLine();
void cmdEnq();
void cmdInit();
void cmdPrnt();
void cmdPrntWo();
void loop();
void writeInt(unsigned int i);
void oldWriteByte(byte b);
void writeByte(byte b);
int CLK_PIN = 2;
int IN_PIN = 3;
int OUT_PIN = 4;
int LED_PIN = 13;

int DELAY = 60;




//byte line[640] = {255,255,255,255,255,255,252,252,252,252,252,252,252,252,252,252,255,255,255,255,255,255,1,1,113,113,120,120,120,120,113,113,255,255,255,255,255,255,255,255,255,255,241,241,192,192,140,140,255,255,255,255,255,255,231,231,231,231,230,230,224,224,99,99,255,255,255,255,255,255,255,255,255,255,126,126,60,60,24,24,255,255,255,255,255,255,254,254,254,254,60,60,12,12,198,198,255,255,255,255,255,255,127,127,127,127,124,124,48,48,115,115,255,255,255,255,255,255,255,255,255,255,127,127,56,56,152,152,255,255,255,255,255,255,255,255,255,255,190,190,32,32,99,99,255,255,255,255,255,255,255,255,255,255,127,127,57,57,25,25,255,255,255,255,255,255,254,254,254,254,252,252,200,200,206,206,255,255,255,255,255,255,115,115,115,115,113,113,32,32,115,115,255,255,255,255,255,255,255,255,255,255,227,227,192,192,156,156,255,255,255,255,255,255,255,255,255,255,252,252,192,192,198,198,255,255,255,255,255,255,243,243,243,243,242,242,48,48,49,49,255,255,255,255,254,254,254,254,255,255,63,63,28,28,156,156,255,255,255,255,79,79,78,78,254,254,30,30,6,6,230,230,255,255,255,255,255,255,115,115,115,115,115,115,115,115,115,115,255,255,255,255,255,255,255,255,255,255,199,199,129,129,57,57,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,252,252,252,252,252,252,252,252,252,252,252,252,255,255,255,255,3,3,113,113,121,121,121,121,121,121,120,120,255,255,255,255,158,158,158,158,158,158,158,158,196,196,192,192,255,255,255,255,103,103,103,103,103,103,99,99,96,96,224,224,255,255,255,255,153,153,153,153,153,153,153,153,24,24,60,60,255,255,255,255,230,230,230,230,230,230,230,230,142,142,14,14,255,255,255,255,99,99,96,96,99,99,99,99,49,49,56,56,255,255,255,255,153,153,25,25,249,249,249,249,25,25,57,57,255,255,255,255,231,231,231,231,231,231,231,231,231,231,231,231,255,255,255,255,153,153,153,153,153,153,153,153,152,152,156,156,255,255,255,255,206,206,206,206,206,206,206,206,142,142,14,14,255,255,255,255,115,115,115,115,115,115,115,115,48,48,56,56,255,255,255,255,156,156,128,128,159,159,159,159,136,136,193,193,255,255,255,255,71,71,79,79,207,207,207,207,207,207,207,207,255,255,255,255,51,51,51,51,51,51,51,51,51,51,51,51,255,255,255,255,153,153,153,153,153,153,152,152,156,156,158,158,255,255,255,255,226,226,242,242,226,226,230,230,70,70,14,14,255,255,255,255,115,115,115,115,115,115,115,115,115,115,115,115,255,255,255,255,56,56,0,0,63,63,63,63,17,17,131,131,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};

byte line[640] = {0,0,0,0,0,0,0,0,0,0,3,3,3,3,3,3,0,0,0,0,0,0,0,0,0,0,99,99,140,140,8,8,0,0,4,4,4,4,4,4,12,12,143,143,140,140,136,136,0,0,0,0,0,0,0,0,0,0,131,131,140,140,216,216,0,0,0,0,64,64,64,64,192,192,193,193,204,204,137,137,0,0,0,0,12,12,8,8,8,8,152,152,152,152,152,152,0,0,0,0,0,0,0,0,0,0,136,136,136,136,136,136,0,0,0,0,0,0,0,0,0,0,102,102,108,108,56,56,0,0,0,0,0,0,0,0,0,0,12,12,100,100,76,76,0,0,0,0,0,0,0,0,4,4,8,8,12,12,12,12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,24,24,200,200,136,136,0,0,0,0,98,98,102,102,70,70,68,68,196,196,196,196,0,0,0,0,0,0,0,0,0,0,40,40,76,76,204,204,0,0,0,0,0,0,0,0,0,0,32,32,33,33,49,49,0,0,128,128,192,192,192,192,128,128,240,240,153,153,137,137,0,0,0,0,196,196,192,192,128,128,140,140,140,140,136,136,0,0,0,0,0,0,0,0,0,0,204,204,100,100,204,204,0,0,32,32,32,32,32,32,32,32,110,110,120,120,112,112,0,0,0,0,0,0,16,16,16,16,120,120,32,32,32,32,3,3,2,2,6,6,6,6,0,0,0,0,0,0,0,0,24,24,25,25,25,25,31,31,1,1,1,1,2,2,12,12,152,152,153,153,153,153,154,154,0,0,0,0,0,0,0,0,152,152,145,145,145,145,31,31,0,0,0,0,0,0,0,0,153,153,158,158,152,152,136,136,0,0,0,0,0,0,0,0,17,17,17,17,49,49,57,57,0,0,0,0,0,0,0,0,152,152,152,152,56,56,217,217,0,0,0,0,0,0,0,0,48,48,48,48,80,80,136,136,0,0,0,0,0,0,0,0,200,200,240,240,192,192,64,64,0,0,0,0,0,0,0,0,22,22,51,51,51,51,19,19,0,0,0,0,0,0,0,0,1,1,1,1,129,129,193,193,0,0,0,0,0,0,0,0,136,136,24,24,24,24,249,249,0,0,0,0,0,0,0,0,204,204,140,140,136,136,140,140,0,0,0,0,0,0,0,0,216,216,240,240,193,193,67,67,0,0,0,0,0,0,0,0,81,81,145,145,155,155,147,147,0,0,0,0,0,0,0,0,25,25,25,25,17,17,35,35,0,0,0,0,0,0,0,0,136,136,24,24,24,24,24,24,0,0,0,0,0,0,0,0,204,204,204,204,136,136,136,136,0,0,0,0,0,0,0,0,112,112,216,216,200,200,140,140,0,0,0,0,0,0,0,0,96,96,96,96,64,64,80,80,0,0,0,0,0,0,0,0};



byte mask = 1;

void setup(){


  Serial.begin(9600); 


  Serial.println("SuperMario"); 


  /* Setup pins */
  pinMode(CLK_PIN, OUTPUT);                                               
  pinMode(OUT_PIN, OUTPUT);                                              
  pinMode(IN_PIN, INPUT); 

  digitalWrite(IN_PIN, HIGH);   // Internal pull-up for the Input port 



  digitalWrite(CLK_PIN, HIGH);  // Reset clock to idle
}


void cmdFin(){
  unsigned int crc = 0;

  /* Line package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x04); // Command Type Enquiry
  crc+= 0x04;

  writeByte(0x00); // Compression. Off

  writeByte(0x00); 
  writeByte(0x00);




  writeInt(crc);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK




}


void cmdLine(){
  unsigned int crc = 0;
  
 /* Line package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x04); // Command Type Enquiry
  crc+= 0x04;
  
  writeByte(0x00); // Compression. Off

  writeByte(0x80); crc+=0x80;
  writeByte(0x02); crc+=0x02;

  for(int i = 0; i< 640; i++){

      byte val = line[i];
      
    
      writeByte(val);
      

      crc += val;
  
  }


  writeInt(crc);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK

  

 
}



void cmdEnq(){

  /* Sample package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x0F); // Command Type Enquiry
  writeByte(0x00); // Compression. Off

  writeByte(0x00);
  writeByte(0x00);

  writeInt(0x0F);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK
}

void cmdInit(){
  /* Init package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x01); // Command Type Enquiry
  writeByte(0x00); // Compression. Off

  writeByte(0x00);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x00);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK
}

void cmdPrnt(){
  /* Start printing package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x02); // Command Type Start Printing
  writeByte(0x00); // Compression. Off

  writeByte(0x04);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x13);

  writeByte(0xe4);
  writeByte(0x40);


  writeByte(0x3e);
  writeByte(0x01);


  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK 
}


void cmdPrntWo(){
  /* Start printing package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x02); // Command Type Start Printing
  writeByte(0x00); // Compression. Off

  writeByte(0x04);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x00); //  margin

  writeByte(0xe4);
  writeByte(0x40);


  writeInt(299);


  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK 
}

void loop(){




  cmdEnq();

  // delay(10000);

  cmdInit();

  cmdLine(); 





  cmdFin();


  cmdPrntWo();

  delay(8000);



  /* Try to read ack */
  byte value1 = 0;

  for(int n = 7; n>=0; n--){
    // delayMicroseconds(DELAY);
    //  digitalWrite(CLK_PIN, LOW);
    // delayMicroseconds(DELAY);

    if( digitalRead(IN_PIN) == HIGH ){
      value1 |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.

    }
    else {
      value1 &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.

    }

    //  digitalWrite(CLK_PIN, HIGH);
    delayMicroseconds(DELAY);

  }



  /* Try to read ack */
  byte value2 = 0;

  for(int n = 7; n>=0; n--){
    //delayMicroseconds(DELAY);
    // digitalWrite(CLK_PIN, LOW);
    // delayMicroseconds(DELAY);

    if( digitalRead(IN_PIN) == HIGH ){
      value2 |= (1 << n);       // forces nth bit of x to be 1.  all other bits left alone.

    }
    else {
      value2 &= ~(1 << n);      // forces nth bit of x to be 0.  all other bits left alone.
    }

    //  digitalWrite(CLK_PIN, HIGH);
    delayMicroseconds(DELAY);



  }

  Serial.println(value1, HEX);
  Serial.println(value2, HEX);
  Serial.println("---");

  //  digitalWrite(CLK_PIN, HIGH);






  delay(1000);

}

void writeInt(unsigned int i){

  writeByte((byte)(i));   // LsB

  writeByte((byte)(i>>8));   // MsB
}

void oldWriteByte(byte b){

  for (mask = 00000001; mask>0; mask <<= 1) { //iterate through bit mask
    if (b & mask){ // if bitwise AND resolves to true
      digitalWrite(OUT_PIN,LOW); // send 1
    }
    else{ //if bitwise and resolves to false
      digitalWrite(OUT_PIN,HIGH); // send 0
    }


    digitalWrite(CLK_PIN, LOW);
    // delayMicroseconds(DELAY);
    //Serial.print(digitalRead(IN_PIN));
    digitalWrite(CLK_PIN, HIGH);
    // delayMicroseconds(DELAY);


  }

}

void writeByte(byte b)
{
  for(int i=0;i<8;i++) {  //NEWS FLASH: there are 8 bits in a byte
    digitalWrite(CLK_PIN,LOW);                            //Set the clock to zero, wes about to send you somethin
    if(b & 0x80) {                                   //Magic
      digitalWrite(OUT_PIN,HIGH);                     //1
    } 
    else {
      digitalWrite(OUT_PIN,LOW);                      //0
    }
    b <<= 1;            //More magic
    delayMicroseconds(DELAY);
    digitalWrite(CLK_PIN,HIGH);                           //Make the clock 1, so we can make a funky squarewave
    delayMicroseconds(DELAY);
  }
  digitalWrite(OUT_PIN,LOW);                          //We are done, turn the serial off

}






int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

